/**************************************************************************(C)*
 *
 *  This file is part of the GreenHills project
 *  Copyright (C) 2009  Tieto Corporation
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  Not for commmercial use. Only with Tieto permission.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 *************************************************************************(C)*/

#include "../gpsposition.h"
#include "util/binstream.h"
#include <stdlib.h>

#if defined Q_OS_LINUX
    #include <math.h>
#elif defined Q_OS_WIN32
    #include <math.h>
#elif defined Q_OS_WINCE
    #define M_PI 3.14159265358979323846
#endif

using namespace greenhills::gps;
using namespace greenhills::util;

CGPSPosition::CGPSPosition()
{}

CGPSPosition::CGPSPosition(const CGPSCoordinate &pLatitude,
        const CGPSCoordinate &pLongitude)
{
    (CGPSCoordinate &)mLatitude = pLatitude;
    (CGPSCoordinate &)mLongitude = pLongitude;
}

QString CGPSPosition::float2string(float pCoordinate,
        char pNegFlg, char pPosFlg)
{
    QString tmp;
    float absCoord = pCoordinate * (pCoordinate<0 ? -1 : 1);
    tmp.sprintf("%02i\260%07.4f",
        abs((int)absCoord)/100, absCoord-((int)absCoord/100)*100);
    tmp += pCoordinate<0 ? pNegFlg : pPosFlg;
    return tmp;
}

QString CGPSPosition::latitude2string(float pLatitude)
{
    return float2string(pLatitude,'S','N');
}

QString CGPSPosition::longitude2string(float pLongitude)
{
    return float2string(pLongitude,'W','E');
}

QString CGPSPosition::toString() const
{
    QString tmp;
    tmp += latitude().toString();
    tmp += " ";
    tmp += longitude().toString();
    return tmp;
}

obinstream& CGPSPosition::operator<<(obinstream &pStream) const
{
    pStream << mLatitude << mLongitude;
    return pStream;
}

ibinstream& CGPSPosition::operator>>(ibinstream &pStream)
{
    pStream >> mLatitude >> mLongitude;
    return pStream;
}

bool CGPSPosition::operator == (
        const CGPSPosition &pCoordinate)
{
    Q_UNUSED(pCoordinate);
    //! @todo Needs some precision or not to store value as float
    return true; //mLatitude == pCoordinate.latitude() &&
           //mLongitude == pCoordinate.longitude();
}

const CGPSPosition& CGPSPosition::operator =(
        const CGPSPosition &pCoordinate)
{
    mLongitude = pCoordinate.longitude();
    mLatitude = pCoordinate.latitude();
    return *this;
}

double centerAngle(const CGPSPosition &point1, const CGPSPosition &point2)
{
    double centerAngle = acos(
            (sin(point1.longitude().toRad()) * sin(point2.longitude().toRad()))+
            (cos(point1.longitude().toRad()) * cos(point2.longitude().toRad()) *
            cos(point2.latitude().toRad() - point1.latitude().toRad())));
    return centerAngle;
}

double CGPSPosition::distanceTo(const CGPSPosition &pTarget) const
{
    double distance = centerAngle(*this, pTarget) * EARTH_DIAMETER;
    return distance;
}

double CGPSPosition::directionTo(const CGPSPosition &pTarget) const
{
  double center = centerAngle(*this,pTarget);
  double x = (sin(pTarget.longitude().toRad()) - sin(this->longitude().toRad())*
              cos(center))/(cos(this->longitude().toRad())*sin(center));

  double y = cos(pTarget.longitude().toRad())*sin(pTarget.latitude().toRad()-
                           this->latitude().toRad())/sin(center);
  double azimut = atan2(y,x);
  if(azimut < 0) azimut = azimut + 2*M_PI;
  azimut = azimut/M_PI*180;
  return (int)azimut;
}

CGPSPosition CGPSPosition::positionTo(double pAngle, double pDistance) const
{
    double u2 =asin(sin(this->longitude().toRad())*
                        cos(pDistance/EARTH_DIAMETER)
                     + cos(this->longitude().toRad())*
                        sin(pDistance/EARTH_DIAMETER)
                     *cos(pAngle*M_PI/180));
    //azimut druheho bodu
    //  double v2 = asin(cos(this.longitude().toRad())*sin(delta_v)/
    //                  sin(pDistance/EARTH_DIAMETER))+180;
    double x =(cos(pDistance/EARTH_DIAMETER)- sin(this->longitude().toRad())
               *sin(u2))/(cos(this->longitude().toRad())
                          *cos(this->longitude().toRad()));
    double y = sin(pDistance/EARTH_DIAMETER)*sin(pAngle/180*M_PI)/cos(u2);
    //    double delta_v = asin(sin(pDistance/EARTH_DIAMETER)*
    //                          sin(pAngle/180*M_PI)/cos(u2));
    double delta_v = atan2(y,x);
    double v2 = delta_v + this->latitude().toRad();
    CGPSPosition position(u2*180/M_PI*CGPSCoordinate::COORD_1_DEGREE,
                          v2*180/M_PI*CGPSCoordinate::COORD_1_DEGREE);
    return position;
}
